/***************************************************************************
 创建者: 华磊
 开始时间: 2019.11.29
 copyright: (C) 华友高科
 修改说明:
 ***************************************************************************
 *  @file x.h
 *  功能:
 * 激光相机标定。
 *
 *                                                                         *
 ***************************************************************************/

#ifndef LASERCAMERACALIBRATE_H
#define LASERCAMERACALIBRATE_H
#include "GeneralDefine.h"
#include <Eigen/Dense>
#include <kdl/frames.hpp>
using namespace Eigen;
using namespace std;
using namespace KDL;

//class PclIcpTransfer;

class LaserCameraCalibrate
{
public:
    LaserCameraCalibrate();

public:
    int updatePclIcpTransferDebugWindow();
    int showPclIcpTransferDebugWindow(bool isShowBaseMap, bool isShowBaseCompareMap,
                              bool isShowLocalMap, bool isShowLocalCompareMap);
    int calculateLaserCameraFrame(std::vector<PointPro> tcpPointList, std::vector<PointPro> cameraPointList,
                                  std::vector<double> &cameraTransferOut, double &maxErrorOut);

    //xyz平移被比例放大了．ｚ不对．
    int calculateTransformMatrix_opencv(std::vector<PointPro> weldInTcpList,
                                 std::vector<PointPro> cameraPointList,
                                    std::vector<float> &cameraTransferOut);

    int test();
private:
    Rotation createEulerMatrix(double psi, double sita, double fai);
    void extractEulerMatrixAngle(const Rotation& rotMat, double& psi, double& sita, double& fai);

    int calculateTransformMatrix_goicp(std::vector<PointPro> weldInTcpList,
                                 std::vector<PointPro> cameraPointList,
                                    std::vector<float> &cameraTransferOut);
    int initialGoicp();
    //有平移，但是没有旋转．
    int calculateTransformMatrix_pciicp(std::vector<PointPro> weldInTcpList,
                                 std::vector<PointPro> cameraPointList,
                                    std::vector<double> &cameraTransferOut);
    int calculateTransformMatrix_geometry(std::vector<PointPro> weldInTcpList,
                                 std::vector<PointPro> cameraPointList,
                                    std::vector<double> &cameraTransferOut);
    int calculateWeldPositionInWorldByCamera(PointPro tcpInWorld, PointPro cameraPoint,
                                             std::vector<double> cameraTransfer, PointPro &weldInWorld);
    int testTransfer(PointPro pointIn,  std::vector<double> cameraTransfer, PointPro &pointOut);
    int calculatePositionInTcp(PointPro weldPointInWorld,PointPro tcpInWorld,PointPro &weldInTcp);
    int getRotationBy3Points(const Matrix3f& rf3, Rotation& rotMat);
    MatrixXf getSchmidtMatrix(const MatrixXf& Sd);

//    PclIcpTransfer *pclicptransfer;


};

#endif // LASERCAMERACALIBRATE_H
